#ifndef _FIFO_MSGQUEUE_H_
#define _FIFO_MSGQUEUE_H_

#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h> 
#include <string.h>

#ifdef __cplusplus
extern "C"{
#endif

#define likely(x)                      __builtin_expect(!!(x), 1)
#define unlikely(x)                    __builtin_expect(!!(x), 0)

#define FIFO_MSGQUEUE_DEBUG

typedef struct bufferMemArea
{
    char     name[32];
    void*    head;
    void*    tail;
    uint32_t size;
    uint32_t elen;             /* element buffer length */
    bool     bmem;             /* allocate element buffer internal? */
    bool     init;
} BUFFER_MEM_AREA, *PBUFFER_MEM_AREA;

typedef struct fifoMsgQueueElem
{
    void*              data;
    uint32_t           size;
} FIFO_MSGQUEUE_EL, *PFIFO_MSGQUEUE_EL;

typedef struct fifoMsgQueueElemPool
{
    uint32_t           size;
    uint32_t           curr;  /* next write position in pool */
    uint32_t           read;  /* next read  position in pool */
    void*              qref; 
#ifdef FIFO_MSGQUEUE_DEBUG
    BUFFER_MEM_AREA    area;  /* storage msgs's memory range */
#endif
    PFIFO_MSGQUEUE_EL  msgs;
    uint32_t           resv[256]; /* protection for overwrite */
} FIFO_MSGQUEUE_EL_POOL, *PFIFO_MSGQUEUE_EL_POOL;

typedef struct fifoMsgQueueStats
{
    uint32_t rx_msgs;
    uint32_t tx_msgs;
    uint32_t w_cycle;
    uint32_t r_cycle;
} FIFO_MSGQUEUE_ST, *PFIFO_MSGQUEUE_ST;

typedef struct fifoMsgQueue
{
    char                  name[32];
    bool                  init;
    FIFO_MSGQUEUE_EL_POOL pool;
    FIFO_MSGQUEUE_ST      stat;
    PBUFFER_MEM_AREA      area;
} FIFO_MSGQUEUE, *PFIFO_MSGQUEUE;

inline void initBufferMemArea(BUFFER_MEM_AREA *area, void *addr, uint32_t size)
{
    area->size = size;
    area->head = addr;
    area->tail = (void*)((uint32_t)area->head + area->size);
    area->init = true;
}

inline bool isPtrInMemArea(BUFFER_MEM_AREA *area, void *ptr)
{
    if( area->bmem || (ptr >= area->head && ptr < area->tail) )
    {
        return true;
    }

    printf("%s, ptr=%p isn't in area=%p(%p-%p)\n", __func__, ptr, area, area->head, area->tail);
    return false;
}

inline void initFifoMsgQueueElemPool(FIFO_MSGQUEUE_EL_POOL *pool, int32_t size)
{
    pool->msgs = (PFIFO_MSGQUEUE_EL)malloc(sizeof(FIFO_MSGQUEUE_EL) * size);
    if( NULL == pool->msgs )
    {
        printf("%s, error: can' alloc memory for fifo pool \n",
            __FUNCTION__);
        return;
    }
    pool->size = size;
    pool->curr = 0;
    pool->read = 0;
    pool->qref = NULL;

#ifdef FIFO_MSGQUEUE_DEBUG
    initBufferMemArea(&pool->area, (void*)pool->msgs, 
        sizeof(FIFO_MSGQUEUE_EL) * size);
#endif

    memset(pool->resv, 0x0, sizeof(pool->resv));
}

inline void freeFifoMsgQueueElemPool(FIFO_MSGQUEUE_EL_POOL *pool)
{
    if( NULL != pool->msgs )
    {
        free(pool->msgs);
        pool->msgs = NULL;
    }
    pool->curr = 0;
    pool->size = 0;
    pool->read = 0;
#ifdef FIFO_MSGQUEUE_DEBUG
    initBufferMemArea(&pool->area, NULL, 0);
    pool->area.init = false;
#endif
}

#if 0
/*
 *  Func:  getFreeFifoMsgQueueElem
 *  Desc:  This primitive is used to get an free element from pool 
 *  Ret:
 *  Note:  might need to protect by lock, !!used it carefully!!
 */
inline PFIFO_MSGQUEUE_EL getFreeFifoMsgQueueElem(FIFO_MSGQUEUE_EL_POOL *pool)
{
    if( pool->curr >= pool->size )
    {   /* for unknow reason, curr is incorrect */
        pool->curr = 0;
    }
    FIFO_MSGQUEUE_EL *elem = &pool->msgs[pool->curr];
    if( pool->curr + 1 >= pool->size )
    {
        pool->curr = 0;
#ifdef FIFO_MSGQUEUE_DEBUG
        FIFO_MSGQUEUE *queue = (FIFO_MSGQUEUE *)pool->qref;
        queue->stat.w_cycle++;
        //printf("%s, reach tail of %s, change to head\n", __func__, queue->name);
#endif
    }
    else
    {
        pool->curr++;
    }
    // printf("%s, get free elem %x\n", __FUNCTION__, elem);
    return elem;
}
#endif

inline int32_t getQueuedMsgsCount(FIFO_MSGQUEUE_EL_POOL *pool)
{
    if( pool->curr >= pool->size || pool->read >= pool->size )
    {
        FIFO_MSGQUEUE *queue = (FIFO_MSGQUEUE *)pool->qref;
#ifdef FIFO_MSGQUEUE_DEBUG
        printf("%s, error: %s has bad data, curr=%d read=%d size=%d\n",
            __func__, queue->name, pool->curr, pool->read, pool->size);
#endif
        return 0;
    }
    
    int32_t count = pool->curr >= pool->read ? pool->curr - pool->read :
        (pool->size - pool->read) + pool->curr;
    if( count > (int32_t)pool->size - 50 )
    {
        FIFO_MSGQUEUE *queue = (FIFO_MSGQUEUE *)pool->qref;
        printf("WARNING: %s is almost out of resource, used %d(%d-%d)/%d \n",
            queue->name, count, pool->curr, pool->read, pool->size);
    }
    return count;
}

/*
 *  Func:  getUsedFifoMsgQueueElem
 *  Desc:  This primitive is used to get an used element from pool 
 *  Ret:
 *  Note:  might need to protect by lock, !!used it carefully!!
 */
inline PFIFO_MSGQUEUE_EL getUsedFifoMsgQueueElem(FIFO_MSGQUEUE_EL_POOL *pool)
{
    if( pool->read >= pool->size )
    {   /* for unknow reason, read is incorrect */
        pool->read = 0;
    }
    FIFO_MSGQUEUE_EL *elem = &pool->msgs[pool->read];
    if( pool->read + 1 >= pool->size )
    {
        pool->read = 0;
#ifdef FIFO_MSGQUEUE_DEBUG
        FIFO_MSGQUEUE *queue = (FIFO_MSGQUEUE *)pool->qref;
        queue->stat.r_cycle++;
        //printf("%s, reach tail of %s, change to head\n", __func__, queue->name);
#endif
    }
    else
    {
        pool->read++;
    }
    // printf("%s, read elem %x\n", __FUNCTION__, elem);
    return elem;
}

#ifdef FIFO_MSGQUEUE_MMSG
inline uint32_t getUsedFifoMsgQueueElems(FIFO_MSGQUEUE_EL_POOL *pool, PFIFO_MSGQUEUE_EL *elems, uint32_t nread)
{
    uint32_t read = nread;

    if( pool->read >= pool->size )
    {   /* unknow reason, read is incorrect */
        pool->read = 0;
    }
    *elems = &pool->msgs[pool->read];

    // If read count > remain elements of pool tail
    // 0 1 2 3 4 5 6 7
    //           r  
    if( read > pool->size - pool->read )
    {   // Limit to read the remain elements of pool tail
        read = pool->size - pool->read;
    }

    int32_t msgCount = getQueuedMsgsCount(pool);
    if( (int32_t)read >= msgCount )
    {   // read >= msgs count in queue
        read = msgCount;
    }
    
    if( pool->read + read >= pool->size )
    {
        pool->read = 0;
#ifdef FIFO_MSGQUEUE_DEBUG
        FIFO_MSGQUEUE *queue = (FIFO_MSGQUEUE *)pool->qref;
        queue->stat.r_cycle++;
        //printf("%s, reach tail of %s, change to head\n", __func__, queue->name);
#endif
    }
    else
    {
        pool->read += read;
    }

    return read;
}
#endif

inline void getMsgFromFifoMsgQueue(FIFO_MSGQUEUE *queue, void **buffer, uint32_t *length)
{
    if( getQueuedMsgsCount(&queue->pool) < 1 )
    {   /* no msg in queue */
        return;
    }

    FIFO_MSGQUEUE_EL *elem = getUsedFifoMsgQueueElem(&queue->pool);
    if( elem->data == NULL || !isPtrInMemArea(queue->area, elem->data) || elem->size < 1 )
    {
#ifdef FIFO_MSGQUEUE_DEBUG
        printf("%s, error: get bad buffer(%p,%d) from %s\n", __func__,
            elem->data, elem->size, queue->name);
#endif
        return;
    }

    *buffer = elem->data;
    *length = elem->size;
}

#ifdef FIFO_MSGQUEUE_MMSG
inline int32_t getMsgsFromFifoMsgQueue(FIFO_MSGQUEUE *queue, PFIFO_MSGQUEUE_EL *elems, int32_t count)
{
    if( getQueuedMsgsCount(&queue->pool) < 1 )
    {   /* no msg in queue */
        return 0;
    }

    int32_t i = 0;
    uint32_t num = getUsedFifoMsgQueueElems(&queue->pool, elems, count);
    FIFO_MSGQUEUE_EL* elem = *((PFIFO_MSGQUEUE_EL*)elems);
    for( ; i < (int32_t)num; ++i )
    {
        if( elem->data == NULL || !isPtrInMemArea(&(queue->pool.area), elem) ||
            !isPtrInMemArea(queue->area, elem->data) )
        {
#ifdef FIFO_MSGQUEUE_DEBUG
            printf("%s, error: get bad buffer(%p,%d) - %d/%d elem(%p) from %s(%d,%d)\n", __func__,
                elem->data, elem->size, i, num, elem, queue->name, queue->pool.curr, queue->pool.read);
#endif
            continue;
        }

        elem += 1;
    }
//    printf("%s, get %d / %d elem from %s\n", __FUNCTION__, num, count, queue->name);
    return num;
}
#endif

inline void addMsgToFifoMsgQueue(FIFO_MSGQUEUE *queue, void *buffer, uint32_t length)
{
    if( buffer == NULL || !isPtrInMemArea(queue->area, buffer) || length < 1 )
    {
#ifdef FIFO_MSGQUEUE_DEBUG
        printf("%s, error: add bad buffer(%p,%d) to %s\n", __func__,
            buffer, length, queue->name);
#endif
        return;
    }

    if( queue->pool.curr >= queue->pool.size )
    {   /* for unknow reason, curr is incorrect */
        queue->pool.curr = 0;
    }

    FIFO_MSGQUEUE_EL *elem = &(queue->pool.msgs[queue->pool.curr]);
    if( queue->area->bmem )
    {
        memcpy(elem->data, buffer, length);
    }
    else
    {
        elem->data = buffer;
    }
    elem->size = length;

    if( queue->pool.curr + 1 >= queue->pool.size )
    {
        queue->pool.curr = 0;
#ifdef FIFO_MSGQUEUE_DEBUG
        queue->stat.w_cycle++;
        //printf("%s, reach tail of %s, change to head\n", __func__, queue->name);
#endif
    }
    else
    {
        queue->pool.curr++;
    }
}

#ifdef FIFO_MSGQUEUE_MMSG
inline int32_t addMsgsToFifoMsgQueue(FIFO_MSGQUEUE *queue, PFIFO_MSGQUEUE_EL elems, int32_t count)
{
    if( elems == NULL || count < 1 )
    {
#ifdef FIFO_MSGQUEUE_DEBUG
        printf("%s, error: incorrect parameters, elems=%p count=%d\n", __func__, elems, count);
#endif
        return 0;
    }

    int32_t put_count = 0, i;

    if( unlikely(queue->pool.curr >= queue->pool.size) )
    {   /* just for unknow reason, curr is incorrect */
        queue->pool.curr = 0;
    }

    for( i = 0; i < count; ++i )
    {
        if( elems[i].data == NULL || !isPtrInMemArea(queue->area, elems[i].data) )
        {
#ifdef FIFO_MSGQUEUE_DEBUG
            printf("%s, error: add bad buffer(%p,%d) - %d/%d elem(%p) to %s\n", __func__,
                elems[i].data, elems[i].size, i, count, &elems[i], queue->name);
#endif
            continue;
        }

        FIFO_MSGQUEUE_EL *elem = &(queue->pool.msgs[queue->pool.curr]);
        elem->data = elems[i].data;
        elem->size = elems[i].size;

        // Note, update curr later than elem    
        if( queue->pool.curr + 1 >= queue->pool.size )
        {
            queue->pool.curr = 0;
#ifdef FIFO_MSGQUEUE_DEBUG
            queue->stat.w_cycle++;
            //printf("%s, reach tail of %s, change to head\n", __func__, queue->name);
#endif
        }
        else
        {
            queue->pool.curr++;
        }

        put_count++;
    }
//    printf("%s, add %d elem to %s\n", __FUNCTION__, count, queue->name);
    return put_count;
}
#endif

/*
*       Fun:   initFifoMsgQueue
*       Desc:  This primitive is used to initialize the queues
*       Ret:
*       Notes: None
*/
inline void initFifoMsgQueue(FIFO_MSGQUEUE *queue, const char *name, int32_t size, bool allocMem, int32_t elemLen)
{
    if( queue->init != true )
    {
        strcpy(queue->name, name);

        initFifoMsgQueueElemPool(&queue->pool, size);
        queue->pool.qref = (void*)queue;

        if( allocMem )
        {
            queue->area = (BUFFER_MEM_AREA*)malloc(sizeof(BUFFER_MEM_AREA));
            if( NULL == queue->area )
            {
                printf("%s, can't malloc queue->area memory \n", __func__);
                return;
            }
            queue->area->elen = elemLen;
            queue->area->bmem = true;
            strcpy(queue->area->name, name);

            void* buffer = malloc(elemLen * size);
            if( NULL == buffer )
            {
                printf("%s, can't malloc %d bytes memory \n", __func__, elemLen * size);
                return;
            }
            initBufferMemArea(queue->area, buffer, elemLen * size);

            int32_t index = 0;
            for( ; index < size; ++index )
            {
                queue->pool.msgs[index].data = (unsigned char *)buffer + elemLen * index;
            }
        }
        else
        {
            queue->area = NULL;
        }

        queue->init = true;
        memset(&queue->stat, 0, sizeof(FIFO_MSGQUEUE_ST));

        printf("%s, %s initialized \n", __func__, name);
    }
}

inline void freeFifoMsgQueue(FIFO_MSGQUEUE *queue)
{
    if( queue->init )
    {
        if( getQueuedMsgsCount(&queue->pool) > 0 )
        {
            //TODO, free the queue data?
        }
        freeFifoMsgQueueElemPool(&queue->pool);
        if( (queue->area != NULL) && queue->area->bmem )
        {
            free(queue->area->head);
            queue->area->head = NULL;
            free(queue->area);
            queue->area = NULL;
            queue->area->bmem = false;
        }
        queue->init = false;
        printf("%s, %s uninitialized \n", __func__, queue->name);
    }
}

#ifdef __cplusplus
}
#endif

#endif //_FIFO_MSGQUEUE_H_
